#ifndef _LOCAL_COSTMAP_H_
#define _LOCAL_COSTMAP_H_

#include <vector>
#include <dispatch_system/moying_map.h>
#include <dispatch_system/vehicle.h>

namespace moying
{
namespace navigation
{

    //局部代价地图
    class LocalCostMap
    {
    public:
        LocalCostMap();

        bool init(MoyingMapPtr moying_map);

        /**
         * @brief 构建机器人的局部代阶地图
         * @param   vehicle           机器人指针
         * @return  返回成功失败
         */ 
        bool buildLocalCostMap(VehiclePtr vehicle);

        /**
         * @brief 获取cell对应的cost
         * @param   mx           cell x坐标
         * @param   my           cell y坐标
         * @return  返回该cell的局部代价
         */
        unsigned char getCellCost(int mx,int my);

        /**
         * @brief 设置cell的cost
         * @param   mx           cell x坐标
         * @param   my           cell y坐标
         * @param   cost        cell的cost
         * @return  返回设置成功或失败
         */
        bool setCellCost(int mx,int my,unsigned char cost);

        const std::vector<std::vector<unsigned char>>& local_cost_map(){return local_cost_map_;}

        int min_cell_x(){return min_cell_x_;}
        int min_cell_y(){return min_cell_y_;}


    private:
        int min_cell_x_; //x维度最小坐标
        int min_cell_y_; //y维度最小坐标
        int max_cell_x_; //x维度最大坐标
        int max_cell_y_; //y维度最大坐标
        std::vector<std::vector<unsigned char>> local_cost_map_;    //局部代价地图
        MoyingMapPtr moying_map_;
    };

    using LocalCostMapPtr = std::shared_ptr<LocalCostMap>;
}
}

#endif